"""
--------------------------------------------------------------------------------
功能:节点管理程序部分，该程序可调用
    1.serial_section:和下位机通信的串口管理部分
    2.servo_section:和下位机通信的舵机协议部分
    3.user_arm:机械臂逆运动学部分
    实现单个程序管理三个节点, 实现控制机械臂运动

日期:2025-09-07
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司.
--------------------------------------------------------------------------------
"""
import time
import math
import cmath

from src.arm_control import serial_section, servo_section, user_arm

class RobotManager:
    def __init__(self):
        #创建并打开和下位机通信串口对象
        self.serial = serial_section.SerialPort(port="/dev/ttyS1", baudrate=115200)
        self.serial.open()
        time.sleep(0.2)
        #创建舵机协议对象
        self.servos = servo_section.ServoProtocol()
        time.sleep(0.2)
        #创建机械臂对象
        self.arm = user_arm.FourDofArm()
        time.sleep(0.2)
        #定义四自由度机械臂参数
        self.angle =     [0.0, 0.0, 0.0, 0.0]
        self.start_ang = [50.0, 0.0, 80.0]   #机械臂逆运动学初始化参数
        self.save_ang =  [50.0, 0.0, 80.0]   #实时保存四自由度机械臂逆运动学角度
        for i in range(3):
            self.save_ang[i] = self.start_ang[i]

        #设置机械臂角度最大值
        self.arm_limit_x_min = 50
        self.arm_limit_x_max = 180
        self.arm_limit_y_min = -80
        self.arm_limit_y_max = 80
        self.arm_limit_z_min = -50
        self.arm_limit_z_max = 140

        self.gimbal_limit_hor_min = 30
        self.gimbal_limit_hor_max = 200
        self.gimbal_limit_ver_min = 30
        self.gimbal_limit_ver_max = 200

        self.arm_move_x = self.start_ang[0]
        self.arm_move_y = self.start_ang[1]
        self.arm_move_z = self.start_ang[2]
        self.gimbal_ang = [140, 140]

    def arm_limit_x(self, input_value_x):
        if input_value_x <= self.arm_limit_x_min:
            input_value_x = self.arm_limit_x_min
        if input_value_x >= self.arm_limit_x_max:
            input_value_x = self.arm_limit_x_max
        return input_value_x

    def arm_limit_y(self, input_value_y):
        if input_value_y <= self.arm_limit_y_min:
            input_value_y = self.arm_limit_y_min
        if input_value_y >= self.arm_limit_y_max:
            input_value_y = self.arm_limit_y_max
        return input_value_y

    def arm_limit_z(self, input_value_z):
        if input_value_z <= self.arm_limit_z_min:
            input_value_z = self.arm_limit_z_min
        if input_value_z >= self.arm_limit_z_max:
            input_value_z = self.arm_limit_z_max
        return input_value_z

    def three_axis_data_limit(self, input_value_x, input_value_y, input_value_z):
        if input_value_x <= self.arm_limit_x_min:
            input_value_x = self.arm_limit_x_min
        if input_value_x >= self.arm_limit_x_max:
            input_value_x = self.arm_limit_x_max

        if input_value_y <= self.arm_limit_y_min:
            input_value_y = self.arm_limit_y_min
        if input_value_y >= self.arm_limit_y_max:
            input_value_y = self.arm_limit_y_max

        if input_value_z <= self.arm_limit_z_min:
            input_value_z = self.arm_limit_z_min
        if input_value_z >= self.arm_limit_z_max:
            input_value_z = self.arm_limit_z_max
        return input_value_x, input_value_y, input_value_z


    """
    机械臂手爪闭合
    """
    def catch_sth(self):
        data = self.servos.servo_control(
            servo_num=1,
            servo_id=[6],
            servo_angle=[110]
        )
        self.serial.serial_send(data)
        time.sleep(0.05)

    """
    机械臂手爪打开
    """
    def release_sth(self):
        data = self.servos.servo_control(
            servo_num=1,
            servo_id=[6],
            servo_angle=[1.01]
        )
        self.serial.serial_send(data)
        time.sleep(0.05)


    """
    机械臂直线插补运动
    机械臂移动函数
    该函数只需要传入机械臂逆运动学的目标位置即可.
    意思就是: 该函数会自动记录机械臂上一次机械臂的目标位置, 
            传入目标位置后, 机械臂会自动从上一次位置运动到新传入的目标位置
    """
    def arm_move(self, target_x, target_y, target_z, input_point):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = self.save_ang[0] - target_x
        delta_y = self.save_ang[1] - target_y
        num_points = 0

        num_points = input_point
        for i in range(num_points + 1):
            t = i / num_points
            x = self.save_ang[0] + t * (target_x - self.save_ang[0])
            y = self.save_ang[1] + t * (target_y - self.save_ang[1])
            z = self.save_ang[2] + t * (target_z - self.save_ang[2])
            x = self.arm_limit_x(x)
            y = self.arm_limit_y(y)
            z = self.arm_limit_z(z)

            current_angle[0] = x
            current_angle[1] = y
            current_angle[2] = z

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(x, y, z)
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang = [current_angle[0], current_angle[1], current_angle[2]]

    """
    机械臂初始化动作
    机械臂从上一次的角度转动到初始化动作
    """
    def arm_init_action(self):
        self.release_sth() #打开爪子
        self.arm_move(self.start_ang[0], self.start_ang[1], self.start_ang[2], 2)

    """
    机械臂圆弧插补
    """
    def arm_cir_move(self, start_x, start_z, end_x, end_z, radius, num_points, reverse):
        ang = [0.0, 0.0, 0.0, 0.0]
        center_x, center_z = self.arm.calculate_circle_center(start_x, start_z, end_x, end_z, radius, reverse)

        if center_x is None:

            return
        start_angle = math.atan2(start_z - center_z, start_x - center_x)
        end_angle = math.atan2(end_z - center_z, end_x - center_x)
        delta_angle = (end_angle - start_angle) / num_points
        for i in range(num_points + 1):
            current_angle = start_angle + i * delta_angle
            x = center_x + radius * math.cos(current_angle)
            z = center_z + radius * math.sin(current_angle)
            y = 0.0
            x = self.arm_limit_x(x)
            y = self.arm_limit_y(y)
            z = self.arm_limit_z(z)

            ang[0],ang[1],ang[2],ang[3] =   self.arm.get_coordinate(x, y, z)

            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[ang[3], ang[2], ang[1], ang[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.005)

            